#ifndef HITSZ_DIP_PRACTICUM_UTILS_H
#define HITSZ_DIP_PRACTICUM_UTILS_H

#include <opencv2/opencv.hpp>
#include <Eigen/Dense>
#include <iostream>
#include <vector>

namespace dip
{
    //BGR
    const uint8_t VISUALIZE_COLOR[][3] = {
        {193,182,255},{144,238,144},{0,69,255}, //粉色、淡绿色、番茄色
        {128,0,128},{0,128,128},{128,128,0}, //紫色、橄榄、水鸭色
        {128,0,0},{0,128,0},{0,0,128},
        {255,0,0},{0,255,0},{0,0,255},
        {255,255,0},{255,0,255},{0,255,255}
    };
    /**
     * @brief 将阈值图像序列可视化为彩色图像
     */ 
    void threshold2rgb(const std::vector<cv::Mat>& threshold_imgs,cv::Mat  &rgb_img)
    {
        if (threshold_imgs.size()<=0){return;}
        const int rows = threshold_imgs[0].rows;
        const int cols = threshold_imgs[0].cols;

        cv::Mat color_mat(rows,cols,CV_8UC3,cv::Scalar(0,0,0));
        for (int i=0;i<threshold_imgs.size();i++)
        {
            const cv::Mat& mask = threshold_imgs[i];
            cv::Mat tmp(rows,cols,CV_8UC3,cv::Scalar(VISUALIZE_COLOR[i][0],VISUALIZE_COLOR[i][1],VISUALIZE_COLOR[i][2]));
            tmp.copyTo(color_mat,mask);
        }

        color_mat.copyTo(rgb_img);
    }

    /**
     * @brief 获取可视化图像
     * @param threshold_imgs,二值化图像
     * @param img_points,图像中提取到的点
     * @param rgb_img,输出图像
     */ 
    void get_visualize_mat(const std::vector<cv::Mat>& threshold_imgs,const std::vector<cv::Point> &img_points,cv::Mat  &rgb_img)
    {
        threshold2rgb(threshold_imgs,rgb_img);
        for (int i=0;i<img_points.size();i++)
        {
            int index = (i+2)/15;
            cv::circle(rgb_img,img_points[i],2,cv::Scalar(VISUALIZE_COLOR[index][0],VISUALIZE_COLOR[index][1],VISUALIZE_COLOR[index][2]));
        }
    }

    /**
     * @brief 将3d点投影到2d
     */ 
    void Proj3DPoints(const std::vector<Eigen::Vector3f> &points3d,std::vector<Eigen::Vector2f> &points2d)
    {
        points2d.clear();
        for (int i=0;i<points3d.size();i++)
        {
            Eigen::Vector2f p;
            p<<points3d[i](0,0),points3d[i](1,0);
            points2d.push_back(p);
        }
    }
}

#endif